/**
 *
 */
package us.oh.k12.wkw.robot.subsystem;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.ADXL345_I2C.AllAxes;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;

/**
 * @author Team4145
 *
 */
public class LaunchFrisbeeSystem extends SystemBase {

    protected static final String SYSTEM_NAME = "Launcher";
    private static final String NETWORKTABLE_NAME_KINECT_COORDINATES = "BFR_COORDINATES";
    private static final String NETWORKTABLE_NAME_KINECT_DISTANCE = "Distance";
    private static final double INITIAL_SHOOTER_MOTOR_SPEED = (double) -0.65;
    // ball shooter motors
    private Talon shootMotor1;
    private Talon shootMotor2;
    private Talon indexMotor;
    private Talon angleMotor;
    private Solenoid plungerRelayExtend;
    private Solenoid plungerRelayRetract;
    PIDController yTargetPidController = null;
    PIDController accelAnglePidController = null;
    private boolean anglePidState = false;
    private ADXL345_I2C adxl = null;
    private int yFilterType = FilterSmoothing.TYPE_ALPHA_BETA;
    private double yFilterAlpha = FilterSmoothing.DEFAULT_ALPHA;
    private double yFilterBeta = FilterSmoothing.DEFAULT_BETA;
    private FilterSmoothing yFilter = null;
    private boolean angleMotorIsOn = false;
    private double angleMotorUpSpeed;
    private double angleMotorDownSpeed;
    private double angleMotorHoldSpeed;
    private double angleMotorSlowDownSpeed;
    private double angleMotorSlowUpSpeed;
    private long angleMotorOnStartTime = 0;
    private long angleMotorTotalOnTime = 0;
    private double previousAngleMotorSpeed = 0;
    private boolean topSwitchOn = false;
    private boolean bottomSwitchOn = false;
    private boolean shootMotor1IsOn = false;
    private boolean shootMotor2IsOn = false;
    // current motor speed (when we turn it on).
    private double shootMotor1Speed;
    private double shootMotor2Speed;
    // tracking how long the motor has been on.
    private long shootMotor1OnStartTime = 0;
    private long shootMotor2OnStartTime = 0;
    private long shootMotor1TotalOnTime = 0;
    private long shootMotor2TotalOnTime = 0;
    private int plungerFireCount = 0;
    private boolean plungerExtended = false;
    private double previousShootMotor1Speed = 0;
    private double previousShootMotor2Speed = 0;
    private double batteryVoltage = 0;
    private double yOffset = 10.0;
    private int accelFilterType = FilterSmoothing.TYPE_ALPHA_BETA;
    private double accelFilterAlpha = FilterSmoothing.DEFAULT_ALPHA;
    private double accelFilterBeta = FilterSmoothing.DEFAULT_BETA;
    private int accelFilterSize = FilterSmoothing.DEFAULT_CIRCULAR_SIZE;
    private double accelFilterFactor = FilterSmoothing.DEFAULT_FACTOR;
    private FilterSmoothing filterX = null;
    private FilterSmoothing filterY = null;
    private FilterSmoothing filterZ = null;
    private int delay = 0;
    private double[] cameraOffsetFootArray = new double[60];
    private double[] angleOffsetFootArray = new double[60];
    private double xRawCurrent = 0;
    //private double xRawLast = 0;
    private double xSmoothCurrent = 0;
    //private double xSmoothLast = 0;
    private double yRawCurrent = 0;
    //private double yRawLast = 0;
    private double ySmoothCurrent = 0;
    //private double ySmoothLast = 0;
    private double zRawCurrent = 0;
    //private double zRawLast = 0;
    private double zSmoothCurrent = 0;
    //private double zSmoothLast = 0;
    private double angleResultCurrent = 0;
    private double angleResultLast = 0;
    private double elevationLast = 0;
    private double elevation = 0;
    private double distanceRaw = 0;
    private int distanceIntRaw = 0;
    private double cameraYraw = 0;
    private double cameraYsmooth = 0;
    private long indexMotorTotalOnTime = 0;
    private long indexMotorStartTime = 0;

    public LaunchFrisbeeSystem() {
        super("LauncherSystem");

        try {

            this.initYfilter();
            this.initMotor();
            this.initAngleMotor();
            this.initPlunger();
            this.initAccelorometer();
            this.initYTargetPid();
            this.initDistance();

            System.out.println("LaunchFrisbeeSystem initialized.");

        } catch (Exception anEx) {
            this.error("LaunchFrisbeeSystem()", anEx);
        }
    }

    private void initYfilter() {

        this.yFilter = new FilterSmoothing(this.yFilterType, this.yFilterAlpha, this.yFilterBeta);

    }

    private void initDistance() {

        // reset all values to 0;
        for (int idx = 0; idx < this.cameraOffsetFootArray.length; idx++) {
            this.cameraOffsetFootArray[idx] = 0;
        }
        for (int idx = 0; idx < this.angleOffsetFootArray.length; idx++) {
            this.angleOffsetFootArray[idx] = 0;
        }

        // hard code specific offset values for each foot in distance away from the target

        this.cameraOffsetFootArray[0] = -5;
        this.angleOffsetFootArray[0] = 0;
        this.cameraOffsetFootArray[1] = -5;
        this.angleOffsetFootArray[1] = 0;
        this.cameraOffsetFootArray[2] = -5;
        this.angleOffsetFootArray[2] = 0;
        this.cameraOffsetFootArray[3] = -5;
        this.angleOffsetFootArray[3] = 0;
        this.cameraOffsetFootArray[4] = -5;
        this.angleOffsetFootArray[4] = 0;
        this.cameraOffsetFootArray[5] = -5;
        this.angleOffsetFootArray[5] = 0;
        this.cameraOffsetFootArray[6] = -2;
        this.angleOffsetFootArray[6] = 0;
        this.cameraOffsetFootArray[7] = -2;
        this.angleOffsetFootArray[7] = 0;
        this.cameraOffsetFootArray[8] = -2;
        this.angleOffsetFootArray[8] = 0;
        this.cameraOffsetFootArray[9] = -2;
        this.angleOffsetFootArray[9] = 0;
        this.cameraOffsetFootArray[10] = -2;
        this.angleOffsetFootArray[10] = 0;
        this.cameraOffsetFootArray[11] = 0;
        this.angleOffsetFootArray[11] = 0;
        this.cameraOffsetFootArray[12] = 0;
        this.angleOffsetFootArray[12] = 0;
        this.cameraOffsetFootArray[13] = 0;
        this.angleOffsetFootArray[13] = 0;
        this.cameraOffsetFootArray[14] = 0;
        this.angleOffsetFootArray[14] = 0;
        this.cameraOffsetFootArray[15] = 2;
        this.angleOffsetFootArray[15] = 0;
        this.cameraOffsetFootArray[16] = 2;
        this.angleOffsetFootArray[16] = 0;
        this.cameraOffsetFootArray[17] = 5;
        this.angleOffsetFootArray[17] = 0;
        this.cameraOffsetFootArray[18] = 5;
        this.angleOffsetFootArray[18] = 0;
        this.cameraOffsetFootArray[19] = 5;
        this.angleOffsetFootArray[19] = 0;
        this.cameraOffsetFootArray[20] = 5;
        this.angleOffsetFootArray[20] = 0;
        this.cameraOffsetFootArray[21] = 10;
        this.angleOffsetFootArray[21] = 0;
        this.cameraOffsetFootArray[22] = 10;
        this.angleOffsetFootArray[22] = 0;
        this.cameraOffsetFootArray[23] = 10;
        this.angleOffsetFootArray[23] = 0;
        this.cameraOffsetFootArray[24] = 10;
        this.angleOffsetFootArray[24] = 0;
        this.cameraOffsetFootArray[25] = 15;
        this.angleOffsetFootArray[25] = 0;
        this.cameraOffsetFootArray[26] = 15;
        this.angleOffsetFootArray[27] = 0;
        this.cameraOffsetFootArray[28] = 15;
        this.angleOffsetFootArray[28] = 0;
        this.cameraOffsetFootArray[29] = 15;
        this.angleOffsetFootArray[29] = 0;
        this.cameraOffsetFootArray[30] = 20;
        this.angleOffsetFootArray[30] = 0;
        this.cameraOffsetFootArray[31] = 20;
        this.angleOffsetFootArray[31] = 0;
        this.cameraOffsetFootArray[32] = 20;
        this.angleOffsetFootArray[32] = 0;
        this.cameraOffsetFootArray[33] = 20;
        this.angleOffsetFootArray[33] = 0;
        this.cameraOffsetFootArray[34] = 20;
        this.angleOffsetFootArray[34] = 0;
        this.cameraOffsetFootArray[35] = 25;
        this.angleOffsetFootArray[35] = 0;
        this.cameraOffsetFootArray[36] = 25;
        this.angleOffsetFootArray[36] = 0;
        this.cameraOffsetFootArray[37] = 25;
        this.angleOffsetFootArray[37] = 0;
        this.cameraOffsetFootArray[38] = 25;
        this.angleOffsetFootArray[38] = 0;
        this.cameraOffsetFootArray[39] = 25;
        this.angleOffsetFootArray[39] = 0;
        this.cameraOffsetFootArray[40] = 30;
        this.angleOffsetFootArray[40] = 0;
        this.cameraOffsetFootArray[41] = 30;
        this.angleOffsetFootArray[41] = 0;
        this.cameraOffsetFootArray[42] = 30;
        this.angleOffsetFootArray[42] = 0;
        this.cameraOffsetFootArray[43] = 30;
        this.angleOffsetFootArray[43] = 0;
        this.cameraOffsetFootArray[44] = 30;
        this.angleOffsetFootArray[44] = 0;
        this.cameraOffsetFootArray[45] = 35;
        this.angleOffsetFootArray[45] = 0;
        this.cameraOffsetFootArray[46] = 35;
        this.angleOffsetFootArray[46] = 0;
        this.cameraOffsetFootArray[47] = 35;
        this.angleOffsetFootArray[47] = 0;
    }

    public String getName() {
        return "LaunchFrisbeeSystem";
    }

    public String getSmartDashboardType() {
        return "LaunchFrisbeeSystem";
    }

    private void initMotor() {

        this.shootMotor1 = new Talon(4);

        LiveWindow.addActuator("LaunchFrisbeeSystem", "ShootMotor1", this.shootMotor1);

        this.shootMotor2 = new Talon(5);
        this.indexMotor = new Talon(6);

        LiveWindow.addActuator("LaunchFrisbeeSystem", "ShootMotor2", this.shootMotor2);

        if (null == this.shootMotor1) {

            this.error("initMotor()", "Shoot motor 1 not found.", null);

        } else if (null == this.shootMotor2) {

            this.error("initMotor()", "Shoot motor 2 not found.", null);

        } else {

            // initialize the motor.

            this.shootMotor1.setExpiration(2.000000);
            this.shootMotor1.setSafetyEnabled(false); // TODO: change to true
            this.shootMotor1.set(0.000000); // make sure the motor is turned off.
            this.shootMotor1.stopMotor();
            this.shootMotor2.setExpiration(2.000000);
            this.shootMotor2.setSafetyEnabled(false); // TODO: change to true
            this.shootMotor2.set(0.000000); // make sure the motor is turned off.
            this.shootMotor2.stopMotor();
            this.shootMotor1IsOn = false;
            this.shootMotor2IsOn = false;
            // set the initial motor speed.
            this.shootMotor1Speed = LaunchFrisbeeSystem.INITIAL_SHOOTER_MOTOR_SPEED;
            this.shootMotor2Speed = LaunchFrisbeeSystem.INITIAL_SHOOTER_MOTOR_SPEED;

            this.shootMotor1OnStartTime = 0;
            this.shootMotor2OnStartTime = 0;

            this.shootMotor1TotalOnTime = 0;
            this.shootMotor2TotalOnTime = 0;

            this.indexMotor.setExpiration(2.000000);
            this.indexMotor.setSafetyEnabled(false);
            this.indexMotor.set(0.000000);
            this.indexMotor.stopMotor();
        }

    }

    private void initAngleMotor() {

        this.angleMotor = new Talon(3);

        LiveWindow.addActuator("LaunchFrisbeeSystem", "AngleMotor", this.angleMotor);

        if (null == this.angleMotor) {

            this.error("initMotor()", "Shoot motor not found.", null);

        } else {

            // initialize the motor.

            this.angleMotor.setExpiration(2.000000);
            this.angleMotor.setSafetyEnabled(false); // TODO: change to true
            this.angleMotor.set(0.000000); // make sure the motor is turned off.
            this.angleMotor.stopMotor();

            this.angleMotorIsOn = false;
            // set the initial motor speed.
            this.angleMotorUpSpeed = -0.325;

            this.angleMotorDownSpeed = 0.175;

            this.angleMotorSlowUpSpeed = -0.25;

            this.angleMotorSlowDownSpeed = 0.15;

            this.angleMotorHoldSpeed = -.01;
            // reset the on time.
            this.angleMotorOnStartTime = 0;
            this.angleMotorTotalOnTime = 0;
        }
    }

    private void initPlunger() {

        this.plungerRelayExtend = new Solenoid(1);

        LiveWindow.addActuator("LaunchFrisbeeSystem", "PlungerRelayExtend", this.plungerRelayExtend);

        this.plungerRelayRetract = new Solenoid(2);

        LiveWindow.addActuator("LaunchFrisbeeSystem", "PlungerRelayRetract", this.plungerRelayRetract);

        this.plungerRelayExtend.set(false);
        this.plungerRelayRetract.set(false);
    }

    private void initYTargetPid() {
        this.yTargetPidController = new PIDController(0.012, 0, 0.0, new YTargetingAnglePidInput(), this.angleMotor);
        this.yTargetPidController.setSetpoint(0);
        this.yTargetPidController.setInputRange(-230, 230);
        this.yTargetPidController.setOutputRange(-0.325, 0.175);
        this.yTargetPidController.setAbsoluteTolerance(2);
        this.yTargetPidController.disable();
    }

    private void initAccelorometer() {

        this.adxl = new ADXL345_I2C(DigitalInput.getDefaultDigitalModule(),
                ADXL345_I2C.DataFormat_Range.k2G);

        this.filterX = new FilterSmoothing(this.accelFilterType,
                this.accelFilterSize, this.accelFilterFactor);
        this.filterY = new FilterSmoothing(this.accelFilterType,
                this.accelFilterSize, this.accelFilterFactor);
        this.filterZ = new FilterSmoothing(this.accelFilterType,
                this.accelFilterSize, this.accelFilterFactor);

    }

    protected void initDefaultCommand() {

        try {
            // no implementation.
        } catch (Exception anEx) {
            this.error("initDefaultCommand()", anEx);
        }
    }

    public void flushStatus() {

        try {

            SmartDashboard.putNumber("AccelFilterType", this.accelFilterType);
            SmartDashboard.putNumber("AccelFilterAlpha", this.accelFilterAlpha);
            SmartDashboard.putNumber("AccelFilterBeta", this.accelFilterBeta);
            SmartDashboard.putNumber("AccelFilterSize", this.accelFilterSize);
            SmartDashboard.putNumber("AccelFilterFactor", this.accelFilterFactor);

            SmartDashboard.putNumber("CameraYfilterType", this.yFilterType);
            SmartDashboard.putNumber("CameraYfilterAlpha", this.yFilterAlpha);
            SmartDashboard.putNumber("CameraYfilterBeta", this.yFilterBeta);

            SmartDashboard.putNumber("CameraYOffset", this.yOffset);

            SmartDashboard.putBoolean("AnglePidEnabled", this.anglePidState);

            SmartDashboard.putNumber("ShootMotor1TotalOnTime", this.shootMotor1TotalOnTime);
            SmartDashboard.putNumber("ShootMotor2TotalOnTime", this.shootMotor2TotalOnTime);

            SmartDashboard.putNumber("IndexMotorTotalOnTime", this.indexMotorTotalOnTime);

            SmartDashboard.putNumber("ShootPlungerFireCount", this.shootPlungerCount);

            SmartDashboard.putNumber("AngleMotorUpSpeed", this.angleMotorUpSpeed);
            SmartDashboard.putNumber("AngleMotorDownSpeed", this.angleMotorDownSpeed);
            SmartDashboard.putNumber("AngleMotorTotalOnTime", this.angleMotorTotalOnTime);

            SmartDashboard.putNumber("CameraYOffset", this.yOffset);

        } catch (Exception anEx) {
            this.error("flushStatus()", anEx);
        }
    }

    public void updateStatus() {

        try {

            if (null != this.shootMotor1) {

                if (Math.abs(this.previousShootMotor1Speed - this.shootMotor1.getSpeed()) > .001) {
                    this.previousShootMotor1Speed = this.shootMotor1.getSpeed();
                    SmartDashboard.putNumber("ShootMotor1Speed", this.shootMotor1.getSpeed());
                }
            }

            if (null != this.shootMotor2) {

                if (Math.abs(this.previousShootMotor2Speed - this.shootMotor2.getSpeed()) > .001) {
                    this.previousShootMotor2Speed = this.shootMotor2.getSpeed();
                    SmartDashboard.putNumber("ShootMotor2Speed", this.shootMotor2.getSpeed());
                }
            }

            if (null != this.angleMotor) {

                if (Math.abs(this.previousAngleMotorSpeed - this.angleMotor.getSpeed()) > .001) {
                    this.previousAngleMotorSpeed = this.angleMotor.getSpeed();
                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());
                }
            }

            if (Math.abs(this.batteryVoltage - DriverStation.getInstance().getBatteryVoltage()) > .01) {
                this.batteryVoltage = DriverStation.getInstance().getBatteryVoltage();
                SmartDashboard.putNumber("BatteryVoltage", DriverStation.getInstance().getBatteryVoltage());
            }

            if (this.plungerExtended != this.getPlungerState()) {
                this.plungerExtended = this.getPlungerState();
                SmartDashboard.putBoolean("PlungerExtended", this.plungerExtended);
            }

            if (this.angleResultLast != this.angleResultCurrent) {

                this.angleResultLast = this.angleResultCurrent;
                SmartDashboard.putNumber("AccelXRaw", this.xRawCurrent);
                SmartDashboard.putNumber("AccelYRaw", this.yRawCurrent);
                SmartDashboard.putNumber("AccelZRaw", this.zRawCurrent);
                SmartDashboard.putNumber("AccelXSmooth", this.xSmoothCurrent);
                SmartDashboard.putNumber("AccelYSmooth", this.ySmoothCurrent);
                SmartDashboard.putNumber("AccelZSmooth", this.zSmoothCurrent);
                SmartDashboard.putNumber("AccelAngleSmooth", this.angleResultCurrent);

            }

            if (this.elevationLast != this.elevation) {

                this.elevationLast = this.elevation;
                SmartDashboard.putNumber("CameraDistanceRaw", this.distanceRaw);
                SmartDashboard.putNumber("CameraDistanceSmooth", this.distanceIntRaw);
                SmartDashboard.putNumber("CameraYRaw", this.cameraYraw);
                SmartDashboard.putNumber("CameraYSmooth", this.cameraYsmooth);
                SmartDashboard.putNumber("ElevationAngle", this.elevation);
            }

        } catch (Exception anEx) {
            this.error("updateStatus()", anEx);
        }
    }

    private class YTargetingAnglePidInput implements PIDSource {

        public double pidGet() {


            return LaunchFrisbeeSystem.this.getCameraElevation();

        }
    }

    public double getCurrentAngle() {

        AllAxes anAllAxes = this.adxl.getAccelerations();

        this.xRawCurrent = anAllAxes.XAxis;
        this.yRawCurrent = anAllAxes.YAxis;
        this.zRawCurrent = anAllAxes.ZAxis;
        this.xSmoothCurrent = this.filterX.filter(this.xRawCurrent);
        this.ySmoothCurrent = this.filterY.filter(this.yRawCurrent);
        this.zSmoothCurrent = this.filterZ.filter(this.zRawCurrent);
        double result = MathUtils.atan(this.ySmoothCurrent / Math.sqrt((this.xSmoothCurrent * this.xSmoothCurrent) + (this.zSmoothCurrent * this.zSmoothCurrent)));
        this.angleResultCurrent = Math.toDegrees(result);

        return this.angleResultCurrent;
    }

    public double getCameraElevation() {

        try {

            this.distanceRaw = SmartDashboard.getNumber("distance");
            this.distanceIntRaw = Double.valueOf(this.distanceRaw).intValue();

            this.cameraYraw = (double) (SmartDashboard.getNumber("y_center"));
            this.cameraYsmooth = this.yFilter.filter(this.cameraYraw);

        } catch (TableKeyNotDefinedException aTkndEx) {
            System.out.println("getCameraElevation() TableKeyNotDefinedException, message=" + aTkndEx.getMessage() + ".");
        }

        if (this.getCurrentAngle() < 52 && this.getCurrentAngle() > -2) {

            this.elevation = (this.cameraYsmooth + this.getYOffset(this.distanceIntRaw));

        } else {

            this.elevation = -this.getYOffset(this.distanceIntRaw);

        }

        return this.elevation;
    }

    private double getYOffset(int pDistance) {
        double aDistanceOffset = 0;

        if ((null != this.cameraOffsetFootArray) && (pDistance > -1) && (pDistance < this.cameraOffsetFootArray.length)) {

            aDistanceOffset = this.cameraOffsetFootArray[pDistance];

        }

        return aDistanceOffset + this.yOffset;
    }

    public void changeYOffset(double pValue) {

        this.yOffset += pValue;
        SmartDashboard.putNumber("CameraYOffset", this.yOffset);

    }

    public void toggleAnglePidEnabled() {

        this.anglePidState = !this.anglePidState;

        if (this.anglePidState && this.getCurrentAngle() < 55 && this.getCurrentAngle() > -2) {
            this.yTargetPidController.enable();
            this.yTargetPidController.setContinuous();
            SmartDashboard.putBoolean("AnglePidEnabled", true);
        } else {
            this.yTargetPidController.disable();
            SmartDashboard.putBoolean("AnglePidEnabled", false);
        }
    }

    public void anglePidDisable() {

        this.yTargetPidController.disable();
        this.anglePidState = false;
        SmartDashboard.putBoolean("AnglePidEnabled", false);
    }

    public void plungerRetract() {
        this.plungerRelayRetract.set(false);
        this.plungerRelayExtend.set(true);
        this.plungerFireCount++;
        this.plungerExtended = true;
        this.shootPlungerCount++;
        SmartDashboard.putBoolean("ShootPlungerFired", true);
    }
    private int shootPlungerCount = 0;

    public void plungerExtend() {
        this.plungerRelayExtend.set(false);
        this.plungerRelayRetract.set(true);
        this.plungerExtended = false;
        SmartDashboard.putBoolean("ShootPlungerFired", false);
    }

    public boolean getPlungerState() {
        return Relay.Value.kOn.equals(this.plungerRelayExtend);
    }

    public boolean angleAboveFeedPoint() {
        boolean isAbove = false;

        if (this.getCurrentAngle() > 31) {
            isAbove = true;
        } else {
            isAbove = false;
        }

        return isAbove;
    }

    public boolean angleAbovePyramidPoint() {
        boolean isAbove = false;

        if (this.getCurrentAngle() > 25.75) {
            isAbove = true;
        } else {
            isAbove = false;
        }

        return isAbove;
    }

    public boolean angleAboveFrontPyramidPoint() {
        boolean isAbove = false;

        if (this.getCurrentAngle() > 37.25) {
            isAbove = true;
        } else {
            isAbove = false;
        }

        return isAbove;
    }

    public boolean angleAboveCornerPyramidPoint() {
        boolean isAbove = false;

        if (this.getCurrentAngle() > 36.5) {
            isAbove = true;
        } else {
            isAbove = false;
        }

        return isAbove;
    }

    public boolean angleAboveOffsetPyramidPoint() {
        boolean isAbove = false;

        if (this.getCurrentAngle() > 45.5) {
            isAbove = true;
        } else {
            isAbove = false;
        }

        return isAbove;
    }

    public void turnOnUpAngle() {

        try {

            // make sure we have the motor object

            if (null != this.angleMotor) {

                if (!this.angleMotorIsOn) {

                    this.angleMotor.set(this.angleMotorUpSpeed);

                    this.angleMotorIsOn = true;

                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());

                    this.angleMotorOnStartTime = System.currentTimeMillis();
                }
            }

        } catch (Exception anEx) {
            this.error("turnOnUpAngle()", anEx);
        }
    }

    public void turnOnDownAngle() {
        try {

            // make sure we have the motor object

            if (null != this.angleMotor) {

                if (!this.angleMotorIsOn) {

                    this.angleMotor.set(this.angleMotorDownSpeed);

                    this.angleMotorIsOn = true;

                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());

                    this.angleMotorOnStartTime = System.currentTimeMillis();
                }
            }

        } catch (Exception anEx) {
            this.error("turnOnDownAngle()", anEx);
        }
    }

    public void turnOnSlowUpAngle() {

        try {

            // make sure we have the motor object

            if (null != this.angleMotor) {

                if (!this.angleMotorIsOn) {

                    this.angleMotor.set(this.angleMotorSlowUpSpeed);

                    this.angleMotorIsOn = true;

                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());

                    this.angleMotorOnStartTime = System.currentTimeMillis();
                }
            }

        } catch (Exception anEx) {
            this.error("turnOnUpAngle()", anEx);
        }
    }

    public void turnOnSlowDownAngle() {
        try {

            // make sure we have the motor object

            if (null != this.angleMotor) {

                if (!this.angleMotorIsOn) {

                    this.angleMotor.set(this.angleMotorSlowDownSpeed);

                    this.angleMotorIsOn = true;

                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());

                    this.angleMotorOnStartTime = System.currentTimeMillis();
                }
            }

        } catch (Exception anEx) {
            this.error("turnOnDownAngle()", anEx);
        }
    }

    public void turnOnHoldAngle() {

        try {

            // make sure we have the motor object

            if (null != this.angleMotor) {

                if (!this.angleMotorIsOn) {

                    this.angleMotor.set(this.angleMotorHoldSpeed);

                    this.angleMotorIsOn = true;

                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());

                    this.angleMotorOnStartTime = System.currentTimeMillis();
                }

            }

        } catch (Exception anEx) {
            this.error("turnOnUpAngle()", anEx);
        }
    }

    public void stopChangingAngle() {

        try {

            // make sure we have a motor object.

            if (null != this.angleMotor) {

                if (this.angleMotorIsOn) {

                    this.angleMotor.set(0.00000000); // set speed at zero

                    this.angleMotor.stopMotor(); // stop the motor

                    this.angleMotorIsOn = false;

                    SmartDashboard.putNumber("AngleMotorSpeed", this.angleMotor.getSpeed());

                    this.angleMotorTotalOnTime += System.currentTimeMillis()
                            - this.angleMotorOnStartTime;

                    this.angleMotorOnStartTime = 0;

                    SmartDashboard.putNumber("AngleMotorOnTime", this.angleMotorTotalOnTime);
                }
            }

        } catch (Exception anEx) {
            this.error("stopChangingAngle()", anEx);
        }
    }

    public void setAngleMotorSpeed(final double pSpeed) {

        try {

            double aSpeed = pSpeed;

            if (aSpeed < -1.000000) {
                aSpeed = -1.000000;
            } else if (aSpeed > 1.000000) {
                aSpeed = 1.000000;
            }

            // save the new speed value.

            this.angleMotorUpSpeed = aSpeed;

            // Doesn't allow the motor to fire in reverse

            if (this.angleMotorUpSpeed > 0.000000) {
                this.angleMotorUpSpeed = 0.000000;
            }

            // do we have a motor object and is the motor on?

            if ((null != this.angleMotor) && (this.angleMotorIsOn)) {

                // set the new speed in the motor.
                this.angleMotor.set(this.angleMotorUpSpeed);

            }

        } catch (Exception anEx) {
            this.error("setAngleMotorSpeed()", anEx);
        }
    }

    public void increaseAngleSpeed(final double pSpeed) {
        this.setAngleMotorSpeed(this.angleMotorUpSpeed - pSpeed);
        this.setAngleMotorSpeed(this.angleMotorDownSpeed + pSpeed);
    }

    public void decreaseAngleSpeed(final double pSpeed) {
        this.setAngleMotorSpeed(this.angleMotorUpSpeed + pSpeed);
        this.setAngleMotorSpeed(this.angleMotorDownSpeed - pSpeed);
    }

    public double getAngleMotorSpeed() {
        return (null == this.angleMotor ? 0 : this.angleMotor.getSpeed());
    }

    public boolean isAngleMotorOn() {
        return this.angleMotorIsOn;
    }

    public long getAngleMotorTotalOnTime() {
        return this.angleMotorTotalOnTime;
    }

    public void turnOnIndexMotor() {

        try {

            this.indexMotor.set(-1.00000);
            this.indexMotorStartTime = System.currentTimeMillis();
            SmartDashboard.putBoolean("IndexMotorOn", true);

        } catch (Exception anEx) {
            this.error("turnOnIndexMotor()", anEx);
        }
    }

    public void turnOffIndexMotor() {

        try {

            this.indexMotor.set(0.00000);
            this.indexMotor.stopMotor();
            this.indexMotorTotalOnTime += System.currentTimeMillis() - this.indexMotorStartTime;
            this.indexMotorStartTime = 0;
            SmartDashboard.putBoolean("IndexMotorOn", false);

        } catch (Exception anEx) {
            this.error("turnOffIndexMotor()", anEx);
        }
    }

    public void turnOnShootMotor() {

        try {

            this.shootMotor1.set(this.shootMotor1Speed);
            this.shootMotor2.set(this.shootMotor2Speed);

            this.shootMotor1IsOn = true;
            this.shootMotor2IsOn = true;

            this.shootMotor1OnStartTime = System.currentTimeMillis();
            this.shootMotor2OnStartTime = System.currentTimeMillis();

            SmartDashboard.putBoolean("ShootMotorOn", true);

        } catch (Exception anEx) {
            this.error("turnOnShootMotor1()", anEx);
        }
    }

    public void turnOffShootMotor() {

        try {

            // make sure we have a motor object.

            this.shootMotor1.set(0.00000000); // set speed at zero
            this.shootMotor2.set(0.00000000); // set speed at zero

            this.shootMotor1.stopMotor(); // stop the motor
            this.shootMotor2.stopMotor(); // stop the motor

            this.shootMotor1IsOn = false;
            this.shootMotor2IsOn = false;

            this.shootMotor1TotalOnTime += System.currentTimeMillis()
                    - this.shootMotor1OnStartTime;
            this.shootMotor2TotalOnTime += System.currentTimeMillis()
                    - this.shootMotor2OnStartTime;

            this.shootMotor1OnStartTime = 0;
            this.shootMotor2OnStartTime = 0;

            SmartDashboard.putBoolean("ShootMotorOn", false);

        } catch (Exception anEx) {
            this.error("turnOffShootMotor1()", anEx);
        }
    }

    public void setShootMotor1Speed(final double pSpeed) {

        try {

            double aSpeed = pSpeed;

            if (aSpeed < -1.000000) {
                aSpeed = -1.000000;
            } else if (aSpeed > 1.000000) {
                aSpeed = 1.000000;
            }

            // save the new speed value.

            this.shootMotor1Speed = aSpeed;

            // Doesn't allow the motor to fire in reverse

            if (this.shootMotor1Speed > 0.000000) {
                this.shootMotor1Speed = 0.000000;
            }

            SmartDashboard.putNumber("ShootMotor1Speed", this.shootMotor1.getSpeed());

            // do we have a motor object and is the motor on?

            if ((null != this.shootMotor1) && (this.shootMotor1IsOn)) {

                // set the new speed in the motor.
                this.shootMotor1.set(this.shootMotor1Speed);

            }

        } catch (Exception anEx) {
            this.error("setShootMotor1Speed()", anEx);
        }
    }

    public void setShootMotor2Speed(final double pSpeed) {

        try {

            double aSpeed = pSpeed;

            if (aSpeed < -1.000000) {
                aSpeed = -1.000000;
            } else if (aSpeed > 1.000000) {
                aSpeed = 1.000000;
            }

            // save the new speed value.

            this.shootMotor2Speed = aSpeed;

            // Doesn't allow the motor to fire in reverse

            if (this.shootMotor2Speed > 0.000000) {
                this.shootMotor2Speed = 0.000000;
            }

            SmartDashboard.putNumber("ShootMotor2Speed", this.shootMotor2.getSpeed());

            // do we have a motor object and is the motor on?

            if ((null != this.shootMotor2) && (this.shootMotor2IsOn)) {

                // set the new speed in the motor.
                this.shootMotor2.set(this.shootMotor2Speed);

            }

        } catch (Exception anEx) {
            this.error("setShootMotor2Speed()", anEx);
        }
    }

    public void increaseMotor1Speed(final double pSpeed) {
        this.setShootMotor1Speed(this.shootMotor1Speed - pSpeed);
    }

    public void increaseMotor2Speed(final double pSpeed) {
        this.setShootMotor2Speed(this.shootMotor2Speed - pSpeed);
    }

    public void decreaseMotor1Speed(final double pSpeed) {
        this.setShootMotor1Speed(this.shootMotor1Speed + pSpeed);
    }

    public void decreaseMotor2Speed(final double pSpeed) {
        this.setShootMotor2Speed(this.shootMotor2Speed + pSpeed);
    }

    public double getShootMotor1Speed() {
        return (null == this.shootMotor1 ? 0 : this.shootMotor1.getSpeed());
    }

    public double getShootMotor2Speed() {
        return (null == this.shootMotor2 ? 0 : this.shootMotor2.getSpeed());
    }

    public boolean isShootMotor1On() {
        return this.shootMotor1IsOn;
    }

    public boolean isShootMotor2On() {
        return this.shootMotor2IsOn;
    }

    public long getShootMotor1TotalOnTime() {
        return this.shootMotor1TotalOnTime;
    }

    public long getShootMotor2TotalOnTime() {
        return this.shootMotor2TotalOnTime;
    }
}
